// --------------------------------------------------------------------------------------------------------------------
// <copyright file="MultiPicknPlaceAgent.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Test.Robotics.Manipulation
{
    using System;
    using System.Collections.Generic;
    using System.Linq;
    using System.Runtime.Serialization;
    using Microsoft.Robotics.Manipulation.PicknPlace;
    using Microsoft.Robotics.Manipulation.Runtime;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Tracing;

    /// <summary>
    /// test agent for re-arranging multi objects from CNC tray to place
    /// on Desk and replacing them on the tray
    /// </summary>
    [DataContract]
    public class MultiPicknPlaceAgent : ConsumerProducerAgent<PicknPlaceStatusMessage, PicknPlaceStatusMessage>,
                                        IControlReceiver<PickAndMoveMessage>,
                                        IControlReceiver<PlaceAndMoveMessage>
    {
        /// <summary>
        /// (single) pick and place agent name
        /// </summary>
        [DataMember(Name = "PickNPlaceAgentName")]
        private string pickNPlaceAgentName;

        /// <summary>
        /// First location on table to place object
        /// </summary>
        [DataMember(Name = "StartPoseOnTable")]
        private Pose startPoseOnTable;

        /// <summary>
        /// Reference arm position used for ordering pick place operations
        /// </summary>
        [DataMember(Name = "armReferencePosition")]
        private Vector3 armReferencePosition;

        /// <summary>
        /// Dimensions of CNC tray in X, Y and Z (Z is ignored because the tray
        /// is horizontal)
        /// </summary>
        [DataMember(Name = "CncLengths")]
        private Vector3 cncLengths;

        /// <summary>
        /// FSM driving pick from task
        /// </summary>
        private SinglePicknPlaceFSM pickFromCNCFSM;

        /// <summary>
        /// FSM driving place task
        /// </summary>
        private SinglePicknPlaceFSM placeToCNCFSM;

        /// <summary>
        /// Pick from CNC FSM context
        /// </summary>
        private SinglePicknPlaceFSMContext pickFromCNCContext;

        /// <summary>
        /// Place to CNC FSM context
        /// </summary>
        private SinglePicknPlaceFSMContext placeToCNCContext;

        /// <summary>
        /// Currently executing FSM context
        /// </summary>
        private SinglePicknPlaceFSMContext currentContext;

        /// <summary>
        /// Control port for single pick and move message to the pick and place agent
        /// </summary>
        private IForwarder<PickAndMoveMessage> singlePickMoveControlPort;

        /// <summary>
        /// Control port for single place and move message to the pick and place agent
        /// </summary>
        private IForwarder<PlaceAndMoveMessage> singlePlaceMoveControlPort;

        /// <summary>
        /// Back end instance that turns mutiple pick and places to sequence of single pick and places
        /// </summary>
        private MultiPicknPlace multiPicknPlace;

        /// <summary>
        /// Initializes a new instance of the <see cref="MultiPicknPlaceAgent" /> class
        /// </summary>
        /// <param name="name">Agent name</param>
        /// <param name="producer">Pick and place agent</param>
        /// <param name="startPoseOnTable">Start pose on table for placing objects</param>
        /// <param name="armReferencePosition">Reference arm position used for ordering pick place operations</param>
        /// <param name="cncLengths">Dimensions of CNC tray (X and Y)</param>
        public MultiPicknPlaceAgent(
            string name,
            IProducer<PicknPlaceStatusMessage> producer,
            Pose startPoseOnTable,
            Vector3 armReferencePosition,
            Vector3 cncLengths)
            : base(name, producer.Name)
        {
            this.pickNPlaceAgentName = producer.Name;
            this.startPoseOnTable = startPoseOnTable;
            this.armReferencePosition = armReferencePosition;
            this.cncLengths = cncLengths;
        }

        /// <summary>
        /// Initializes the agent
        /// </summary>
        /// <param name="locator">Agent locator to use</param>
        public override void Initialize(AgentLocator locator)
        {
            base.Initialize(locator);

            IAgentHost pickPlaceAgentHost = locator.GetAgent(this.pickNPlaceAgentName);
            this.singlePickMoveControlPort = pickPlaceAgentHost.GetControlPort<PickAndMoveMessage>();
            this.singlePlaceMoveControlPort = pickPlaceAgentHost.GetControlPort<PlaceAndMoveMessage>();

            this.multiPicknPlace = new MultiPicknPlace(this.startPoseOnTable, this.armReferencePosition, this.cncLengths);
        }

        /// <summary>
        /// Called when agent is activated.
        /// </summary>
        public override void OnActivated()
        {
            base.OnActivated();
            this.pickFromCNCContext = null;
            this.pickFromCNCFSM = null;
            this.placeToCNCContext = null;
            this.placeToCNCFSM = null;
            this.currentContext = null;
        }

        /// <summary>
        /// Handles pick and move from CNC
        /// </summary>
        /// <param name="pickMessage">Pick and move message </param>
        public void ReceiveControl(PickAndMoveMessage pickMessage)
        {
            TraceOut.Info(TraceContexts.Manip, "MultiPickFrom Request with {0} objects", pickMessage.State.Length);

            if (null == this.pickFromCNCFSM && 
                (null == this.placeToCNCContext || this.placeToCNCContext.SingleOperationStatus == PicknPlaceStatusEnum.PlaceComplete))
            {
                if (pickMessage.State.Length > 1)
                {
                    this.ExecutePickFromCNC(pickMessage.State);
                }
                else if (pickMessage.State.Length == 1)
                {
                    // Old style single pick and place
                    this.placeToCNCFSM = null;
                    this.pickFromCNCContext = null;
                    this.currentContext = null;
                    this.PostPickAndMove(pickMessage.State[0]);
                }
            }
        }

        /// <summary>
        /// Handles place and move from CNC
        /// </summary>
        /// <param name="placeMessage">Place and move message </param>
        public void ReceiveControl(PlaceAndMoveMessage placeMessage)
        {
            TraceOut.Info(TraceContexts.Manip, "MultiPlace to Request with {0} objects", placeMessage.State.Length);

            if (placeMessage.State.Length <= 0)
            {
                TraceOut.Warning(TraceContexts.Manip, "Requires at least one place location found {0}", placeMessage.State.Length);
                return;
            }

            if (null == this.placeToCNCContext && (null != this.pickFromCNCContext && this.pickFromCNCContext.OverallContextStatus == PicknPlaceStatusEnum.PickComplete))
            {
                this.ExecutePlaceToCNC(placeMessage.State);
            }
            else if (null == this.currentContext && null == this.pickFromCNCContext && null == this.placeToCNCContext)
            {
                // Old style single pick and place
                this.placeToCNCFSM = null;
                this.PostPlaceAndMove(placeMessage.State[0]);
            }
        }

        /// <summary>
        /// Receives status from Pick and place agent
        /// </summary>
        /// <param name="message">Pick and place status</param>
        public override void Receive(PicknPlaceStatusMessage message)
        {
            if (null != this.currentContext)
            {
                PicknPlaceStatusEnum overallStatus = PicknPlaceStatusEnum.NotStarted;

                this.currentContext.SingleOperationStatus = message.StatusEnum;
                overallStatus = this.currentContext.OverallContextStatus;

                // Want to use current time as originating time for down stream components
                PicknPlaceStatusMessage overallStatusMsg = new PicknPlaceStatusMessage(
                                new AgentMessage(AgentMessage.GetCurrentTime()),
                                string.Format("Multi operation {0}", overallStatus.ToString()),
                                overallStatus,
                                message.ManipulatedObjectDescription,
                                message.PickPlacePose,
                                message.RetractPose);

                this.Publisher.Post(overallStatusMsg);

                if (null != this.pickFromCNCFSM)
                {
                    this.pickFromCNCFSM.Step();
                }
                else if (null != this.placeToCNCFSM)
                {
                    this.placeToCNCFSM.Step();
                }
            }
            else
            {
                // Just republish the message
                PicknPlaceStatusMessage msgCopy = new PicknPlaceStatusMessage(
                                message,
                                message.StatusString,
                                message.StatusEnum,
                                message.ManipulatedObjectDescription,
                                message.PickPlacePose,
                                message.RetractPose);

                this.Publisher.Post(msgCopy);
            }
        }

        /// <summary>
        /// Posts a pick and move to the (single) pick and place agent
        /// </summary>
        /// <param name="pickMove">Pick and move request</param>
        public void PostPickAndMove(PickAndMove pickMove)
        {
            this.singlePickMoveControlPort.Post(new PickAndMoveMessage(AgentMessage.GetCurrentTime(), new PickAndMove[] { pickMove }));
        }

        /// <summary>
        /// Posts a place and move to the (single) pick and place agent
        /// </summary>
        /// <param name="placeMove">Place and move request</param>
        public void PostPlaceAndMove(PlaceAndMove placeMove)
        {
            this.singlePlaceMoveControlPort.Post(new PlaceAndMoveMessage(AgentMessage.GetCurrentTime(), new PlaceAndMove[] { placeMove }));
        }

        /// <summary>
        /// Executes a set pick from CNC and place on table requests 
        /// </summary>
        /// <param name="pickFromCNC">Input array of requests</param>
        private void ExecutePickFromCNC(PickAndMove[] pickFromCNC)
        {
            this.pickFromCNCContext = new SinglePicknPlaceFSMContext();
            this.multiPicknPlace.ExecutePickFromCNC(pickFromCNC, this.pickFromCNCContext);

            this.currentContext = this.pickFromCNCContext;
            this.pickFromCNCFSM = new SinglePicknPlaceFSM(
                        this.pickFromCNCContext, 
                        this.PostPickAndMove, 
                        this.PostPlaceAndMove, 
                        PicknPlaceStatusEnum.PickInProgress,
                        PicknPlaceStatusEnum.PickComplete);

            this.placeToCNCContext = null;
            this.placeToCNCFSM = null;
        }

        /// <summary>
        /// Executes a set place to CNC from previously place on table commands
        /// </summary>
        /// <param name="placeToCNC">Input array of requests</param>
        private void ExecutePlaceToCNC(PlaceAndMove[] placeToCNC)
        {
            this.placeToCNCContext = new SinglePicknPlaceFSMContext();
            this.multiPicknPlace.ExecutePlaceToCNC(placeToCNC, this.pickFromCNCContext, this.placeToCNCContext);
            this.pickFromCNCContext = null;
            this.pickFromCNCFSM = null;

            this.currentContext = this.placeToCNCContext;
            this.placeToCNCFSM = new SinglePicknPlaceFSM(
                this.placeToCNCContext, 
                this.PostPickAndMove, 
                this.PostPlaceAndMove, 
                PicknPlaceStatusEnum.PlaceInProgress,
                PicknPlaceStatusEnum.PlaceComplete);
        }
    }
}
